/******************************************************************
Copyright © LibKz 2017~2023. All rights reserved.
* @projectName   LibKz
* @author        Raiiwhen
* @email         masterraii@icloud.com
* @origin        \06_Car\01_TJCat\01_TJ_Cat_WheelSpeedLoop\ALGO
*******************************************************************/

#ifndef _PID_H_
#define _PID_H_

#include "motor.h"
#include "math.h"

typedef struct{
	float kP,kI,kD;
	float P, I, D;
	float last_err;
	float I_max, I_min;
	float exp, real;
	float deadzone;
	u16 dt;
}PID;

extern PID paw_vel[4];
extern PID paw_ang[4];
extern PID paw_mom[4];

void PID_Init(PID* obj, float kP, float kI, float kD, float deadzone, u16 dt);
float PID_Update(PID* obj, float expect, float real);
void PID_Reset(PID* obj);

#endif
